from pi.controller.moveController import moveController
from pi.controller.sensorController import sensorController
from pi.controller.ultrasonicServoController import ultrasonicServoController
# from pi.controller.buzzerController import buzzerController
from pi.controller.infraredController import infraredController
from pi.model.instruction import instruction
from pi.model.packet import packet
from pi.model.data import data
from pi.config import *
import socket
import time
import threading

if __name__ == '__main__':
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    address_s = (addr_BaseStation_1, port_BaseStation_1)
    sock.connect(address_s)
    print("成功连接到基站", sock)

    # instruction_stack = []  # 指令栈，原来存储历史指令，每个元素都是instruction类型
    trace_stack = []  # 实际运行路径（实际运行的指令 = 服务器下发 + 避障）

    # con_move = moveController(trace_stack=trace_stack)
    # con_ult_servo = ultrasonicServoController()
    # con_infra = infraredController()

    # 一个独立的线程发送传感器数据
    # threading.Thread(target=send_back_data, args=(sock,)).start()
    # send_back_data(sock)

    _sock = sock
    print("进入传数据线程")
    sensorCon = sensorController()  # todo：传感器列表从哪来 ？
    sensor_data = data()  # 建一个空的data
    sensor_data.set_type(data.DATA_TRACE_AND_TRACE)  # 设置为 传感器类型
    while True:
        # todo:所有传感器都关闭，还要不要反馈数据 ？
        sensor_data.set_data_list(sensorCon.get_data_all())  # 获取并 填充数据
        sensor_data.set_trace_stack(trace_stack)
        trace_stack.clear()  # todo:是否需要清空 ？ 是否需要保留历史记录 ？发送失败怎么办 ？
        _mypac = packet(src_id="", des_id="",
                        pac_type=2, ctl_type=1,
                        content=sensor_data,
                        )  # 实例化一个报文对象，并填充数据
        print(sensorCon.get_data_all())
        print(_mypac)
        _sock.send(_mypac.to_bin())
        time.sleep(2)
